/***************************************************************************
 创建者: 华磊
 开始时间: 2016.11.28
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
１）华磊，　2017.06.21　　把接口重新整理了一遍，以前在motorInterface的单位转换的函数去掉，集成到这里。
 ***************************************************************************
 *  @file jointdecouple.cpp
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#include "jointdecouple.h"
#include <qdebug.h>
#include "messagelog.h"
#include "filteringalgorithm.h"

JointDecouple::JointDecouple(QVector<JointAttribute> *robotAttributeIn, int robotTypeIn, int robotIdIn, double deltaTimeIn):
    robotAttribute(robotAttributeIn),
    robotType(robotTypeIn)
{
    deltaTime=deltaTimeIn;
    jointNum = robotAttribute->size();

    //未解偶的命令值。
    positionCommand.resize(jointNum);//位置命令
    velCommand.resize(jointNum);//速度命令
    velCompensation.resize(jointNum);

    accCommand.resize(jointNum);//速度命令
    torqueCommand.resize(jointNum);//力矩命令
    torqueCompensation.resize(jointNum);
    //初始化为０
    for(int i=0;i<jointNum;i++)
    {
        velCompensation[i]=0;
        torqueCompensation[i]=0;
    }



    // 解偶后的命令值。
    positionCommandDecoupled.resize(jointNum);//位置命令
    velCommandDecoupled.resize(jointNum);//速度命令
    velCompensationDecoupled.resize(jointNum);
    accCommandDecoupled.resize(jointNum);//速度命令
    torqueCommandDecoupled.resize(jointNum);//力矩命令
    torqueCompensationDecoupled.resize(jointNum);

    //未解偶,反馈
    positionFeedbackOriginal.resize(jointNum);//反馈位置
    velFeedbackOriginal.resize(jointNum);//反馈速度
    accFeedbackOriginal.resize(jointNum);
    torqueFeedbackOriginal.resize(jointNum);//反馈力矩

    //已经解偶,反馈
    positionFeedbackDecoupled.resize(jointNum);//反馈位置
    velFeedbackDecoupled.resize(jointNum);//反馈速度
    accFeedbackDecoupled.resize(jointNum);
    torqueFeedbackDecoupled.resize(jointNum);//反馈力矩

    absoluteZeroMotorValue.resize(jointNum);
    jointMechanicalZeroValue.resize(jointNum);

    //初始化０值
    motorDataCommandVector=new QVector<MotorData>();
    motorDataFeedbackVector=new QVector<MotorData>();
    motorDataCommandVector->resize(jointNum);
    motorDataFeedbackVector->resize(jointNum);
    MotorData data;
    data.acceleration=0;
    data.position=0;
    data.torque=0;
    data.speed=0;
    for(int i=0;i<jointNum;i++)
    {
        (*motorDataCommandVector)[i]=data;
        (*motorDataFeedbackVector)[i]=data;
        absoluteZeroMotorValue[i]=0;
        jointMechanicalZeroValue[i]=0;
        filtering_feedback.append(new FilteringAlgorithm);
    }

//    dataAnalysizerForJointCommand=new DataAnalysizerDouble("dataAnalysizerForJointCommand!", 0, 5000, 4*jointNum,4000, 2);
//    dataAnalysizerForMotorCommand=new DataAnalysizer("dataAnalysizerForMotorCommand!", 0, 5000, 4*jointNum,4000, 2);
//    dataAnalysizerForJointFeedback=new DataAnalysizerDouble("dataAnalysizerForJointFeedback!", 0, 5000, 4*jointNum,4000, 2);
    //    dataAnalysizerForMotorFeedback=new DataAnalysizer("dataAnalysizerFormotorDataFeedbackVector!", 0, 5000, 4*jointNum,4000, 2);
}

int JointDecouple::setAxisRatedTorque(int axisIdIn, double rateTorqueIn)
{
    if(axisIdIn<0||axisIdIn>=(*robotAttribute).size())
    {
        return -1;
    }
    (*robotAttribute)[axisIdIn].motorRatedTorque=rateTorqueIn;
    return 1;
}

int JointDecouple::getMotorEncoderPulsePerRound(QVector<double> &pulseOut)
{
    //todo
}


int JointDecouple::translateJointToPulse()
{//ok

    for(int i=0; i<jointNum; i++)
    {
        if( (*robotAttribute)[i].jointType==0 )//旋转关节
        {
            //电机编码器值＝（关节角度／360）×减速比× 电机每圈的脉冲数
            (*motorDataCommandVector)[i].position =absoluteZeroMotorValue[i]+
                    (*robotAttribute)[i].motorDirection*(*robotAttribute)[i].pulsePerRound * (positionCommandDecoupled[i]-jointMechanicalZeroValue[i])
                    * (*robotAttribute)[i].gearRatio /360.0;
            //电机速度＝（关节角度速度／360）×减速比× 电机每圈的脉冲数
            (*motorDataCommandVector)[i].speed =  (*robotAttribute)[i].motorDirection*(*robotAttribute)[i].pulsePerRound *velCommandDecoupled[i] * (*robotAttribute)[i].gearRatio /360.0 ;
            (*motorDataCommandVector)[i].velocityOffset =  (*robotAttribute)[i].motorDirection*(*robotAttribute)[i].pulsePerRound
                    *velCompensationDecoupled[i] * (*robotAttribute)[i].gearRatio /360.0 ;
            //电机加速度＝（关节角加速度／360）×减速比× 电机每圈的脉冲数
            (*motorDataCommandVector)[i].acceleration = (*robotAttribute)[i].motorDirection*(*robotAttribute)[i].pulsePerRound * accCommandDecoupled[i] * (*robotAttribute)[i].gearRatio /360.0 ;
            //电机力矩＝１０００×关节力矩／减速比/电机额定力矩
            (*motorDataCommandVector)[i].torque =(*robotAttribute)[i].motorDirection* 1000*torqueCommandDecoupled[i] / (*robotAttribute)[i].gearRatio/(*robotAttribute)[i].motorRatedTorque;
            (*motorDataCommandVector)[i].torqueOffset =(*robotAttribute)[i].motorDirection* 1000*torqueCompensationDecoupled[i] / (*robotAttribute)[i].gearRatio/(*robotAttribute)[i].motorRatedTorque;
//            qDebug()<<"velCommandDecoupled[i]   "<<velCommandDecoupled[i]<<"(*robotAttribute)[i].gearRatio   "<<(*robotAttribute)[i].gearRatio
//                   <<"(*robotAttribute)[i].pulsePerRound   "<<(*robotAttribute)[i].pulsePerRound<<"(*motorDataCommandVector)[i].speed  "
//                  <<(*motorDataCommandVector)[i].speed;
        }
        else//移动关节
        {
            //电机编码器值＝关节位移／螺距 ×减速比× 电机每圈的脉冲数
            (*motorDataCommandVector)[i].position =absoluteZeroMotorValue[i]+
                    (*robotAttribute)[i].motorDirection*(*robotAttribute)[i].pulsePerRound * (positionCommandDecoupled[i]-jointMechanicalZeroValue[i])
                    * (*robotAttribute)[i].gearRatio / ((*robotAttribute)[i].screwPitch) ;
            //电机速度＝关节位移速度／螺距 ×减速比× 电机每圈的脉冲数
            (*motorDataCommandVector)[i].speed = (*robotAttribute)[i].motorDirection*(*robotAttribute)[i].pulsePerRound * velCommandDecoupled[i] * (*robotAttribute)[i].gearRatio / ((*robotAttribute)[i].screwPitch);
            (*motorDataCommandVector)[i].velocityOffset = (*robotAttribute)[i].motorDirection*(*robotAttribute)[i].pulsePerRound * velCompensationDecoupled[i] * (*robotAttribute)[i].gearRatio / ((*robotAttribute)[i].screwPitch);
            //电机加速度＝关节位移加速度／螺距 ×减速比× 电机每圈的脉冲数
            (*motorDataCommandVector)[i].acceleration = (*robotAttribute)[i].motorDirection*(*robotAttribute)[i].pulsePerRound * accCommandDecoupled[i] * (*robotAttribute)[i].gearRatio / ((*robotAttribute)[i].screwPitch);
            //电机力矩＝1000*螺距x关节位移力矩／(2pix减速比)/电机额定力矩
            (*motorDataCommandVector)[i].torque =   (*robotAttribute)[i].motorDirection*1000*((*robotAttribute)[i].screwPitch) *torqueCommandDecoupled[i] / ( (*robotAttribute)[i].gearRatio*(2*M_PI) )/(*robotAttribute)[i].motorRatedTorque ;
            (*motorDataCommandVector)[i].torqueOffset =   (*robotAttribute)[i].motorDirection*1000*((*robotAttribute)[i].screwPitch) *torqueCompensationDecoupled[i] / ( (*robotAttribute)[i].gearRatio*(2*M_PI) )/(*robotAttribute)[i].motorRatedTorque ;
        }
    }
    return 1;
}


int JointDecouple::translatePulseToJoint()
{
    for(int i=0; i<jointNum;i++)
    {
        if( 0 == (*robotAttribute)[i].jointType )//旋转关节
        {
            //关节角度 = 360* 电机编码器值/电机每圈的脉冲数/减速比
            positionFeedbackOriginal[i] = jointMechanicalZeroValue[i]
                    +(*robotAttribute)[i].motorDirection*360/ (*robotAttribute)[i].gearRatio * ( (*motorDataFeedbackVector)[i].position-absoluteZeroMotorValue[i])
                    / (*robotAttribute)[i].pulsePerRound;
            //关节角速度 = 360 * 电机速度/电机每圈的脉冲数/减速比
            velFeedbackOriginal[i] = (*robotAttribute)[i].motorDirection*360 / (*robotAttribute)[i].gearRatio * (*motorDataFeedbackVector)[i].speed / (*robotAttribute)[i].pulsePerRound;
            //关节加速度 = 360 * 电机加速度/电机每圈的脉冲数/减速比
            accFeedbackOriginal[i] = (*robotAttribute)[i].motorDirection*360 / (*robotAttribute)[i].gearRatio * (*motorDataFeedbackVector)[i].acceleration/ (*robotAttribute)[i].pulsePerRound;
            //关节力矩＝(电机力矩/1000)*电机额定力矩*减速比
            torqueFeedbackOriginal[i]=(*robotAttribute)[i].motorDirection*(*motorDataFeedbackVector)[i].torque*(*robotAttribute)[i].motorRatedTorque * (*robotAttribute)[i].gearRatio/1000.0;
            //qDebug()<<"   positionFeedbackOriginal[i]  "<<i<<"==="<<positionFeedbackOriginal[i];
            //qDebug()<< "J1 VELssssssssssssssss333333===" <<  velFeedbackOriginal(0)<<"(*motorDataFeedbackVector)[i].speed" <<(*motorDataFeedbackVector)[i].speed;
        }
        else//移动关节
        {
            //关节长度 = 螺矩 * 电机编码器值/电机每圈的脉冲数/减速比
            positionFeedbackOriginal[i] = jointMechanicalZeroValue[i]
                    +(*robotAttribute)[i].motorDirection*(*robotAttribute)[i].screwPitch / (*robotAttribute)[i].gearRatio *
                    ( (*motorDataFeedbackVector)[i].position-absoluteZeroMotorValue[i]) / (*robotAttribute)[i].pulsePerRound;
            //关节速度 = 螺矩 * 电机速度/电机每圈的脉冲数/减速比
            velFeedbackOriginal[i] = (*robotAttribute)[i].motorDirection*(*robotAttribute)[i].screwPitch / (*robotAttribute)[i].gearRatio * (*motorDataFeedbackVector)[i].speed / (*robotAttribute)[i].pulsePerRound;
            //关节加速度 = 螺矩 * 电机加速度/电机每圈的脉冲数/减速比
            accFeedbackOriginal[i] = (*robotAttribute)[i].motorDirection*(*robotAttribute)[i].screwPitch / (*robotAttribute)[i].gearRatio * (*motorDataFeedbackVector)[i].acceleration / (*robotAttribute)[i].pulsePerRound;
            //关节位移力矩=(电机力矩/1000)*电机额定力矩x2PIx减速比/螺距
            torqueFeedbackOriginal[i]=(*robotAttribute)[i].motorDirection*(*motorDataFeedbackVector)[i].torque *(*robotAttribute)[i].motorRatedTorque*2.0 * M_PI
                    * (*robotAttribute)[i].gearRatio/(*robotAttribute)[i].screwPitch/1000.0;
        }
    }
    return 1;
}


int JointDecouple::initialDataAnalysizer()
{
//    dataAnalysizerForJointCommand->resetDataForAnalysize();
//    dataAnalysizerForJointFeedback->resetDataForAnalysize();
//    dataAnalysizerForMotorCommand->resetDataForAnalysize();
//    dataAnalysizerForMotorFeedback->resetDataForAnalysize();
    return 1;
}


int JointDecouple::decoupleByOrder(QVector<int> deOrder)
{//ok
    int tmpJointNum=deOrder.size();
    int i;
    for(int j=0; j<tmpJointNum; j++)
    {
        i=deOrder[j];
        if((*robotAttribute)[i].coupleFlag)//该轴受其他轴耦合作用
        {
            if(IF_DEBUG)
            {
                qDebug() << " JointDecouple::decouple====关节解耦进行中";
            }
            int coupleNum = (*robotAttribute)[i].coupleJoint;
            if(coupleNum>=tmpJointNum)
            {
                qDebug() << "ERROR ,coupleNum>=tmpJointNum,JointDecouple::reverseDecouple"
                         << i+1;
                addMsg(ENUM_MSG_ERROR,"ProgramEngine","ProgramEngine",8500,robotId,i+1);
               return -1;

            }
            if( ( 0==(*robotAttribute)[i].jointType) &&
                    ( 0==(*robotAttribute)[coupleNum].jointType ) )//该轴为关节运动,且耦合轴也为关节
            {
                //关节耦合补偿后的角度＝关节原来角度＋耦合关节的角度/耦合减速比
                positionCommandDecoupled[i] =positionCommand[i]
                        +(positionCommandDecoupled[coupleNum]-jointMechanicalZeroValue[i]) / (*robotAttribute)[i].coupleReducer;
                //关节耦合补偿后的速度＝关节原来速度＋耦合关节的速度/耦合减速比
                velCommandDecoupled[i] = velCommand[i]+velCommandDecoupled[coupleNum] / (*robotAttribute)[i].coupleReducer;
                velCompensationDecoupled[i] = velCompensation[i]+velCompensationDecoupled[coupleNum] / (*robotAttribute)[i].coupleReducer;
                //关节耦合补偿后的加速度＝关节原来加速度＋耦合关节的加速度/耦合减速比
                accCommandDecoupled[i] = accCommand[i] +accCommandDecoupled[coupleNum] / (*robotAttribute)[i].coupleReducer;
                //关节耦合补偿后的力＝关节原来力＋耦合关节的力/耦合减速比
                torqueCommandDecoupled[i] = torqueCommand[i] +torqueCommandDecoupled[coupleNum] / (*robotAttribute)[i].coupleReducer;
                torqueCompensationDecoupled[i] = torqueCompensation[i] +torqueCompensationDecoupled[coupleNum] / (*robotAttribute)[i].coupleReducer;
            }
            else if( ( 1==(*robotAttribute)[i].jointType ) &&
                     ( 0==(*robotAttribute)[coupleNum].jointType ) )//该轴为直线运动,且耦合轴为关节
            {
                //直线耦合补偿后的长度＝直线原来长度＋（耦合关节的角度／360）×　螺距/耦合减速比
                positionCommandDecoupled[i] =positionCommand[i]
                        + (positionCommandDecoupled[coupleNum]-jointMechanicalZeroValue[i])  / 360.0 *(*robotAttribute)[i].screwPitch/ (*robotAttribute)[i].coupleReducer;
                //直线耦合补偿后的速度＝直线原来速度＋（耦合关节的角度速度／360）×　螺距/耦合减速比
                 velCommandDecoupled[i] = velCommand[i]
                         +velCommandDecoupled[coupleNum]  / 360.0 * (*robotAttribute)[i].screwPitch/ (*robotAttribute)[i].coupleReducer;
                 velCompensationDecoupled[i] = velCompensation[i]
                         +velCompensationDecoupled[coupleNum]  / 360.0 * (*robotAttribute)[i].screwPitch/ (*robotAttribute)[i].coupleReducer;
                //直线耦合补偿后的加速度＝直线原来加速度＋（耦合关节的角度加速度／360）×　螺距/耦合减速比
               accCommandDecoupled[i] = accCommand[i]
                       +accCommandDecoupled[coupleNum] / 360.0 * (*robotAttribute)[i].screwPitch/ (*robotAttribute)[i].coupleReducer ;
               //直线耦合补偿后的力＝直线原来力＋耦合关节力矩x2PI/螺距/耦合减速比
              torqueCommandDecoupled[i] = torqueCommand[i]
                       +torqueCommandDecoupled[coupleNum]*2*M_PI/(*robotAttribute)[i].screwPitch/(*robotAttribute)[i].coupleReducer;
              torqueCompensationDecoupled[i] = torqueCompensation[i]
                       +torqueCompensationDecoupled[coupleNum]*2*M_PI/(*robotAttribute)[i].screwPitch/(*robotAttribute)[i].coupleReducer;

            }
            else
            {
                qDebug() << "不支持的关节解耦类型";
                return -1;//不支持解耦类型
            }
        }
        else
        {

            //关节耦合补偿后的角度＝关节原来角度
            positionCommandDecoupled[i] =positionCommand[i];
            //关节耦合补偿后的速度＝关节原来速度
            velCommandDecoupled[i] = velCommand[i];
            velCompensationDecoupled[i] = velCompensation[i];
            //关节耦合补偿后的加速度＝关节原来加速度
            accCommandDecoupled[i] = accCommand[i] ;
            //关节耦合补偿后的力＝关节原来力
            torqueCommandDecoupled[i] = torqueCommand[i] ;
            torqueCompensationDecoupled[i] = torqueCompensation[i] ;
        }
    }



    return 1;
}

int JointDecouple::reverseDecoupleByOrder(QVector<int> deOrder)
{//ok
    int tmpJointNum=deOrder.size();
    int i;
    for(int j=0; j<tmpJointNum; j++)
    {
        i=deOrder[j];
        if((*robotAttribute)[i].coupleFlag)//该轴受其他轴耦合作用
        {
            if(IF_DEBUG)
            {
                qDebug() << "JointDecouple::reverseDecouple====关节反解耦"<< i;
            }
            int coupleNum = (*robotAttribute)[i].coupleJoint;
            if(coupleNum>=tmpJointNum)
            {
               qDebug() << "ERROR ,coupleNum>=tmpJointNum,JointDecouple::reverseDecouple"
                        << i+1;
               addMsg(ENUM_MSG_ERROR,"ProgramEngine","ProgramEngine",8500,robotId,i+1);
               return -1;

            }
            if( (0==(*robotAttribute)[i].jointType) &&
                    (0==(*robotAttribute)[coupleNum].jointType) )//该轴为关节运动,且耦合轴也为关节
            {
                //关节原来角度 = 关节耦合补偿后的角度 - 耦合关节的角度/耦合减速比
                positionFeedbackDecoupled[i] =positionFeedbackOriginal[i]
                        - (positionFeedbackDecoupled[coupleNum]-jointMechanicalZeroValue[i])/ (*robotAttribute)[i].coupleReducer;
                //关节原来速度 = 关节耦合补偿后的速度 - 耦合关节的速度/耦合减速比
                velFeedbackDecoupled[i] =velFeedbackOriginal[i] - velFeedbackDecoupled[coupleNum] / (*robotAttribute)[i].coupleReducer;
                //关节原来加速度 = 关节耦合补偿后的加速度 - 耦合关节的加速度/耦合减速比
                accFeedbackDecoupled[i] =accFeedbackOriginal[i] - accFeedbackDecoupled[coupleNum] / (*robotAttribute)[i].coupleReducer;
                //关节原来力= 关节耦合补偿后的力 - 耦合关节的力/耦合减速比
                torqueFeedbackDecoupled[i] =torqueFeedbackOriginal[i] - torqueFeedbackDecoupled[coupleNum] / (*robotAttribute)[i].coupleReducer;
            }
            else if( (1==(*robotAttribute)[i].jointType) &&
                     (0==(*robotAttribute)[coupleNum].jointType) )//该轴为直线运动,且耦合轴为关节
            {
                //关节原来长度 = 关节耦合补偿后的长度 -（耦合关节的角度／360）× 螺距/耦合减速比
                positionFeedbackDecoupled[i] =positionFeedbackOriginal[i]
                        - (positionFeedbackDecoupled[coupleNum]-jointMechanicalZeroValue[i]) * (*robotAttribute)[i].screwPitch / 360 /  (*robotAttribute)[i].coupleReducer;
                //关节原来速度 = 关节耦合补偿后的速度 -（耦合关节的角度速度／360）×　螺距/耦合减速比
                velFeedbackDecoupled[i] =velFeedbackOriginal[i]
                        - velFeedbackDecoupled[coupleNum] *  (*robotAttribute)[i].coupleReducer / 360/ (*robotAttribute)[i].screwPitch;
                //关节原来加速度 = 关节耦合补偿后的加速度 - （耦合关节的角度速度／360）×　螺距/耦合减速比
                accFeedbackDecoupled[i] =accFeedbackOriginal[i] -
                        - accFeedbackDecoupled[coupleNum] *  (*robotAttribute)[i].coupleReducer / 360/ (*robotAttribute)[i].screwPitch;
                //关节原来力 = 关节耦合补偿后的力 - 耦合关节力矩x2PI/螺距/耦合减速比
                torqueFeedbackDecoupled[i] =torqueFeedbackOriginal[i]
                        - torqueFeedbackDecoupled[coupleNum] *2*M_PI/(*robotAttribute)[i].screwPitch/(*robotAttribute)[i].coupleReducer;


            }
            else
            {
                qDebug() << "不支持的关节解耦类型";
                return -1;//不支持类型
            }
        }
        else
        {
            //关节原来长度 = 关节耦合补偿后的长度
            positionFeedbackDecoupled[i] =positionFeedbackOriginal[i] ;
            //关节原来速度 = 关节耦合补偿后的速度
            velFeedbackDecoupled[i] =velFeedbackOriginal[i] ;
            //关节原来加速度 = 关节耦合补偿后的速加度
            accFeedbackDecoupled[i] =accFeedbackOriginal[i] ;
            //关节原来力 = 关节耦合补偿后的力
            torqueFeedbackDecoupled[i] =torqueFeedbackOriginal[i] ;

            //qDebug()<< "J1 VELssssssssssssssss333333===" <<  velFeedbackDecoupled(0) ;
        }
    }

    return 1;
}


int JointDecouple::decouple()
{
    QVector<int> tmpDeOrder;
    tmpDeOrder.resize(jointNum);
    switch (robotType)
    {
    //四轴，后面关节对前面的关节存在耦合影响，所以正向解偶从后往前计算
        case ENUM_HEXPOD_6AXIS:
        case ENUM_UR_6AXIS:
        case ENUM_DELTA_ROBOT_4Axis:
        case ENUM_DESCART_ROBOT:
        case ENUM_DESCART_XYZR_ROBOT:
        case ENUM_SCARA:
        {
            for(int j=0;j<jointNum;j++)
            {
                   tmpDeOrder[j]=jointNum-1-j;
            }
            break;
        }
        //正向解偶，从前往后计算
        case ENUM_DESCART_WRIST_ROBOT:
        case ENUM_DESCART_WRAC_ROBOT:
        case ENUM_DESCART_WRACTEST_ROBOT:
        case ENUM_6AXIS_CHAIN:
        {
            for(int j=0;j<jointNum;j++)
            {
                    tmpDeOrder[j]=j;
            }
            break;
        }
        case ENUM_7AXIS:
        {
            for(int j=0;j<jointNum;j++)
            {
                    tmpDeOrder[j]=j;
            }
            break;
        }
        case ENUM_JOINT_ROBOT:
        {
            for(int j=0;j<jointNum;j++)
            {
                    tmpDeOrder[j]=j;
            }
            break;
        }

        default:
        {
            qDebug() << "不支持的机器人类型进行解耦---" << "robotType:" << robotType;
            return -1;
        }
    }

    return decoupleByOrder(tmpDeOrder);

}

int JointDecouple::reverseDecouple()
{
    QVector<int> tmpDeOrder;
    tmpDeOrder.resize(jointNum);
    switch (robotType)
    {
    //反解偶计算顺序与正向解偶相反
    case ENUM_HEXPOD_6AXIS:
    case ENUM_UR_6AXIS:
    case ENUM_DELTA_ROBOT_4Axis:
        case ENUM_DESCART_ROBOT:
        case ENUM_DESCART_XYZR_ROBOT:
        case ENUM_SCARA:
        {
            for(int j=0;j<jointNum;j++)
            {
                    tmpDeOrder[j]=j;
            }
            break;
        }
        case ENUM_DESCART_WRIST_ROBOT:
        case ENUM_DESCART_WRAC_ROBOT:
        case ENUM_DESCART_WRACTEST_ROBOT:
        case ENUM_6AXIS_CHAIN:
        {
            for(int j=0;j<jointNum;j++)
            {
                   tmpDeOrder[j]=jointNum-1-j;
            }
            break;
        }
        case ENUM_7AXIS:
        {
            for(int j=0;j<jointNum;j++)
            {
                   tmpDeOrder[j]=jointNum-1-j;
            }
            break;
        }
        case ENUM_JOINT_ROBOT:
        {
            for(int j=0;j<jointNum;j++)
            {
                   tmpDeOrder[j]=jointNum-1-j;
            }
            break;
        }

        default:
        {
            qDebug() << "不支持的机器人类型进行解耦---" << "robotType:" << robotType;
            return -1;
        }
    }
    return reverseDecoupleByOrder(tmpDeOrder);
}

int  JointDecouple::setDecoupleData(const QVector<double> &jointsPosition, const QVector<double> &jointsVel, const QVector<double> &jointAcc, const QVector<double> &jointTorque)
 {
     positionCommand=jointsPosition;
     velCommand=jointsVel;
     accCommand=jointAcc;
     torqueCommand=jointTorque;
     return 1;

}

int JointDecouple::setDecoupleData_velCompensation(const QVector<double> &jointsVelCompensation)
{
    velCompensation=jointsVelCompensation;
    return 1;
}

//int JointDecouple::setDecoupleData_qqVel(const QVector<double> &qqVelIn)
//{

//}

int JointDecouple::setDecoupleData_torqueCompensation(const QVector<double> &jointsTorqueCompensation)
{
    torqueCompensation=jointsTorqueCompensation;
    return 1;
}

 int JointDecouple::executeDecoupleToPulse()
 {
     //解耦
     if(decouple()<0)
     {
         return -1;
     }
//     qDebug()<<"positionCommand(0)"<<velCommand(0);
//     qDebug()<<"positionCommandDecoupled(0)"<<velCommandDecoupled(0);

     //单位换算
     translateJointToPulse();

//     qDebug()<<"(*motorDataCommandVector)[i].position"<<(*motorDataCommandVector)[0].speed;

     //记录JointCommandDataVector
     std::vector<double> tmpDataVector;
     tmpDataVector.resize(4*jointNum);
     for(int i=0;i<jointNum;i++)
     {
         tmpDataVector[i*4+0]=positionCommand[i] ;
         tmpDataVector[i*4+1]=velCommand[i] ;
         tmpDataVector[i*4+2]=accCommand[i] ;
         tmpDataVector[i*4+3]=torqueCommand[i] ;
     }
//     dataAnalysizerForJointCommand->recordDataForAnalysize(tmpDataVector);

     //记录JointFeedbackDataVector
     for(int i=0;i<jointNum;i++)
     {
         tmpDataVector[i*4+0]=positionFeedbackDecoupled[i] ;
         tmpDataVector[i*4+1]=velFeedbackDecoupled[i] ;
         tmpDataVector[i*4+2]=accFeedbackDecoupled[i];
         tmpDataVector[i*4+3]=torqueFeedbackDecoupled[i] ;
     }
//     dataAnalysizerForJointFeedback->recordDataForAnalysize(tmpDataVector);

     //记录motorCommandDataVector
     std::vector<int> tmpDataVectorInt;
     tmpDataVectorInt.resize(4*jointNum);
     for(int i=0;i<jointNum;i++)
     {
         tmpDataVectorInt[i*4+0]=(*motorDataCommandVector)[i].position;
         tmpDataVectorInt[i*4+1]=(*motorDataCommandVector)[i].speed;
         tmpDataVectorInt[i*4+2]=(*motorDataCommandVector)[i].acceleration;
         tmpDataVectorInt[i*4+3]=(*motorDataCommandVector)[i].torque;
     }
//     dataAnalysizerForMotorCommand->recordDataForAnalysize(tmpDataVectorInt);
     //记录motorDataFeedbackVectorDataVector
     for(int i=0;i<jointNum;i++)
     {
         tmpDataVectorInt[i*4+0]=(*motorDataFeedbackVector)[i].position;
         tmpDataVectorInt[i*4+1]=(*motorDataFeedbackVector)[i].speed;
         tmpDataVectorInt[i*4+2]=(*motorDataFeedbackVector)[i].acceleration;
         tmpDataVectorInt[i*4+3]=(*motorDataFeedbackVector)[i].torque;
     }
//     dataAnalysizerForMotorFeedback->recordDataForAnalysize(tmpDataVectorInt);

     return 1;
 }

 int JointDecouple::getFeedbackJoint(QVector<double> &jointsPosition, QVector<double> &jointsVel, QVector<double> &jointAcc, QVector<double> &jointTorque)
 {
     jointsPosition=positionFeedbackDecoupled;
     jointsVel=velFeedbackDecoupled;
     jointTorque=torqueFeedbackDecoupled;
     jointAcc=accFeedbackDecoupled;
     //qDebug()<< "J1 VELssssssssssssssss222222222222===" <<  jointsVel(0) ;
     return 1;

 }

 int JointDecouple::getCommandJoint(QVector<double> &jointsPosition, QVector<double> &jointsVel, QVector<double> &jointAcc, QVector<double> &jointTorque)
 {

     jointsPosition=positionCommand;
     jointsVel=velCommand;
     jointTorque=torqueCommand;
     jointAcc=accCommand;
     //qDebug()<< "J1 VELssssssssssssssss222222222222===" <<  jointsVel(0) ;
     return 1;

 }
 int JointDecouple::executeReverseDecoupleToJoint()
 {
     translatePulseToJoint();
     if(reverseDecouple()<0)
     {
         return -1;
     }
     return 1;
 }

 QVector<MotorData>  JointDecouple::getMotorDataCommandVector()
 {
     //911MotorTest
//     static int positionCommand_old[20];
//     static int tmpLoopCount=0;
//     if(tmpLoopCount<1000)
//     {
//         tmpLoopCount++;
//     }
     //先屏蔽掉位置之外的数据 todo
     for(int i=0;i<jointNum;i++)
     {
         (*motorDataCommandVector)[i].acceleration=0;
//         (*motorDataCommandVector)[i].speed=0;//agv内部使用速度。不得赋值为０．
         (*motorDataCommandVector)[i].torque=0;
//         (*motorDataCommandVector)[i].torqueOffset=0;
//         (*motorDataCommandVector)[i].velocityOffset=0;
//         double tmpVelOffset=0;
//         if(tmpLoopCount>10)//避免第一次使能时，计算不对．
//         {
//            tmpVelOffset=((double)((*motorDataCommandVector)[i].position-positionCommand_old[i]))/deltaTime;
//         }
//         (*motorDataCommandVector)[i].velocityOffset=filtering_feedback[i]->maxAbsValueFilter(tmpVelOffset,5);
//         if(1==i)
//         {
//              qDebug()<<"feedVel"<<tmpVelOffset<<",velOffset_encoder"<<(*motorDataCommandVector)[i].velocityOffset
//                     <<"velCompensation_joint"<<velCompensation[i];
//         }
         //使用伺服内部的位置速度前馈时，多点关节平滑的过渡点衔接可能有问题．皮带跟踪的过渡点衔接也会有问题．
         //使用内部位置速度前馈，需要解决的问题是，从使能关，电机动了后，在使能开，这个衔接怎么处理．

         //普通速度补偿＋位置前馈
//         (*motorDataCommandVector)[i].velocityOffset+=filtering_feedback[i]->arithmeticMeanfilter(tmpVelOffset,6);
//         (*motorDataCommandVector)[i].velocityOffset=filtering_feedback[i]->medianMean(tmpVelOffset,5);
//         (*motorDataCommandVector)[i].velocityOffset=tmpVelOffset;
         //跟踪衔接部位、关节平滑衔接部位，速度前馈可能不连续，速度前馈突然降低接近０．

//         positionCommand_old[i]=(*motorDataCommandVector)[i].position;
     }


     return *motorDataCommandVector;
 }

 QVector<MotorData>  JointDecouple::getMotorDataFeedbackVector()
 {
     return *motorDataFeedbackVector;
 }


 int JointDecouple::setMotorFeedbackDataVector(QVector<MotorData> motorFeedbackIn)
 {
   (*motorDataFeedbackVector)=motorFeedbackIn;
     executeReverseDecoupleToJoint();
   return 1;
 }

 int JointDecouple::recordCurrentPosition2AbsoluteZeroMotorValue(QVector<int>& returnZeroMotorData)
 {
     returnZeroMotorData.resize(jointNum);
     for(int i=0;i<jointNum;i++)
     {
        absoluteZeroMotorValue[i]=(*motorDataFeedbackVector)[i].position;
     }
     returnZeroMotorData=absoluteZeroMotorValue;
     return 1;
 }

 int JointDecouple::recordCurrentPosition2AbsoluteZeroMotorValue(int& returnZeroMotorData, int axisId)
 {
     if(axisId>=jointNum || axisId<0)
     {
         qDebug()<<"setAbsoluteZeroMotorValue error, jointValue.size()!=jointNum";
         return -1;
     }

    absoluteZeroMotorValue[axisId]=(*motorDataFeedbackVector)[axisId].position;
    returnZeroMotorData=absoluteZeroMotorValue[axisId];

     return 1;
 }

 int JointDecouple::setAbsoluteZeroMotorValue(QVector<int> zeroMotorData)
 {
     if(zeroMotorData.size()!=jointNum)
     {
         qDebug()<<"setAbsoluteZeroMotorValue error, jointValue.size()!=jointNum";
         return -1;
     }
    absoluteZeroMotorValue=zeroMotorData;
     return 1;
 }

 int JointDecouple::setJointMechanicalZeroValue(QVector<double> jointValue)
 {
     if(jointValue.size()!=jointNum)
     {
         qDebug()<<"setJointMechanicalZeroValue error, jointValue.size()!=jointNum";
         return -1;
     }
     jointMechanicalZeroValue=jointValue;
     return 1;
 }

 int JointDecouple::translateJointToPulse(int axisId, double jointValueIn)
 {
     double tmpReturn;

     if( (*robotAttribute)[axisId].jointType==0 )//旋转关节
     {
         //电机编码器值＝（关节角度／360）×减速比× 电机每圈的脉冲数
         tmpReturn =(*robotAttribute)[axisId].motorDirection*(*robotAttribute)[axisId].pulsePerRound * (jointValueIn)
                 * (*robotAttribute)[axisId].gearRatio /360.0;

     }
     else//移动关节
     {
         //电机编码器值＝关节位移／螺距 ×减速比× 电机每圈的脉冲数
         tmpReturn =(*robotAttribute)[axisId].motorDirection*(*robotAttribute)[axisId].pulsePerRound * (jointValueIn)
                 * (*robotAttribute)[axisId].gearRatio / ((*robotAttribute)[axisId].screwPitch) ;
     }
    qDebug()<<"tmpReturn"<<tmpReturn;
     return tmpReturn;
 }


 void JointDecouple::addMsg(int messageLevel, string componentName, string messageType,
                            int messageCode, int robotId,
                            int parameter1, int parameter2, int parameter3, int parameter4,QString message)
 {
     Message tmpMsg;
     tmpMsg.MessageLevel = messageLevel;
     tmpMsg.componentClassName = componentName;
     tmpMsg.messageType = messageType;
     tmpMsg.robotId = robotId;
     tmpMsg.messageCode = messageCode;
    // tmpMsg.time = t1.tv_sec;


     QString infomationStr;

     switch(messageCode)
     {

     case 8500:
     {
         infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
         infomationStr = infomationStr +
                 QObject::tr("机器人轴(%1)耦合轴不存在！").arg(parameter1)
                 .arg(message);
         break;


     }

     default:
     {

     }
     }


     tmpMsg.MessageInformation = infomationStr.toStdString();

     // qDebug()<<"53444444444444444444444"<< tmpMsg.messageCode;
     MessageLog::getInstance()->addMessage(tmpMsg);
 }


